using UnityEngine;
using System.Collections.Generic;
using FPLibrary;

namespace PathX
{
    public static class PathExtension
    {
        /// <summary>
        /// Gets the nearest walkable cell to a position.
        /// </summary>
        /// <param name="grid">The grid.</param>
        /// <param name="position">The position to initiate from.</param>
        /// <param name="inRelationTo">The position of approach.</param>
        /// <param name="requireWalkableFromPosition">Whether the cell to find must be accessible from the direction of <paramref name="position"/></param>
        /// <param name="maxCellDistance">The maximum cell distance to check.</param>
        /// <param name="unitProps">The unit properties.</param>
        /// <returns>The first walkable cell in the neighbourhood of <paramref name="position"/> that is the closest to <paramref name="inRelationTo"/>. If no such cell is found, null is returned.</returns>
        public static Cell GetNearestWalkableCell(this IGrid grid, FPVector position, FPVector inRelationTo, bool requireWalkableFromPosition, int maxCellDistance, IUnitProperties unitProps)
        {
            var cell = grid.GetCell(position);
            if (cell == null)
            {
                return null;
            }

            if (cell.IsWalkableWithClearance(unitProps))
            {
                return cell;
            }

            int dist = 1;
            var candidates = new List<Cell>();
            while (candidates.Count == 0 && dist <= maxCellDistance)
            {
                foreach (var c in grid.GetConcentricNeighbours(cell, dist++))
                {
                    if (requireWalkableFromPosition)
                    {
                        if (c.IsWalkableFromWithClearance(cell, unitProps))
                        {
                            candidates.Add(c);
                        }
                    }
                    else if (c.IsWalkableWithClearance(unitProps))
                    {
                        candidates.Add(c);
                    }
                }
            }

            Cell winner = null;
            Fix64 lowestDist = float.MaxValue;
            for (int i = 0; i < candidates.Count; i++)
            {
                var distSqr = (candidates[i].position - inRelationTo).sqrMagnitude;
                if (distSqr < lowestDist)
                {
                    winner = candidates[i];
                    lowestDist = distSqr;
                }
            }

            return winner;
        }
    }
}